import os
from ament_index_python import get_package_share_directory
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution


def generate_launch_description():
    """
    读取目标二维码, 并发布tf与odom
    """

    package_path = get_package_share_directory("robot_define")
    urdf_file = os.path.join(package_path, "urdf", "robot_simple.urdf")
    with open(urdf_file, "r") as infp:
        urdf_content = infp.read()

    state_publisher_node = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher_node",
        output="screen",
        parameters=[
            {"robot_description": urdf_content},
            {"frame_prefix": PathJoinSubstitution([LaunchConfiguration("prefix"), " "])},
        ],
    )

    odom2tf = Node(
        package="toolbox",
        executable="odom2tf",
        name="odom2tf",
        output="screen",
        remappings=[("odom", "sp_odom")],
        parameters=[{"tf_name": "chassis"}],
    )

    tagFinder = Node(
        package="april_tag_finder",
        executable="finder_auto",
        name="tag_finder",
        remappings=[
            ("camera_image", "camera/image_color"),
            ("camera_info", "camera/camera_info"),
        ],
        parameters=[
            {"debug": True},
            {"tag_size": 0.04},
            {"tag_family": "tag36h11"},
        ],
        output="screen",
    )

    tag2chassis = Node(
        package="april_tag_finder",
        executable="tag2chassis",
        name="tag2chassis",
        output="screen",
        parameters=[
            {"frame_prefix": LaunchConfiguration("prefix")},
            {"target_prefix": LaunchConfiguration("prefix")},#Attentaion 假设各车同构
        ],
    )

    return LaunchDescription(
        [
            DeclareLaunchArgument("prefix", default_value=""),
            state_publisher_node,
            odom2tf,
            tagFinder,
            tag2chassis,
        ]
    )
